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Abstract 

Background: In a deforming anatomic environment, tlie motion of an instrument 
suffers from complex geometrical and dynamic constraints, robot assisted minimally 
invasive surgery therefore requires more sophisticated skills for surgeons. This paper 
proposes a novel dynamic virtual fixture (DVF) to enhance the surgical operation 
accuracy of admittance-type medical robotics in the deforming environment. 

Methods: A framework for DVF on the Euclidean Group 5^3) is presented, which 
unites rotation and translation in a compact form. First, we constructed the 
holonomic/non-holonomic constraints, and then searched for the corresponded 
reference to make a distinction between preferred and non-preferred directions. 
Second, different control strategies are employed to deal with the tasks along the 
distinguished directions. The desired spatial compliance matrix is synthesized from 
an allowable motion screw set to filter out the task unrelated components from 
manual input, the operator has complete control over the preferred directions; while 
the relative motion between the surgical instrument and the anatomy structures is 
actively tracked and cancelled, the deviation relative to the reference is compensated 
jointly by the operator and DVF controllers. The operator, haptic device, admittance-type 
proxy and virtual deforming environment are involved in a hardware-in-the-loop 
experiment, human-robot cooperation with the assistance of DVF controller is 
carried out on a deforming sphere to simulate beating heart surgery, performance 
of the proposed DVF on admittance-type proxy is evaluated, and both human 
factors and control parameters are analyzed. 

Results: The DVF can improve the dynamic properties of human-robot cooperation 
in a low-frequency (0-40 rad/sec) deforming environment, and maintain synergy 
of orientation and translation during the operation. Statistical analysis reveals that 
the operator has intuitive control over the preferred directions, human and the DVF 
controller jointly control the motion along the non-preferred directions, the target 
deformation is tracked actively. 

Conclusions: The proposed DVF for an admittance-type manipulator is capable of 
assisting the operator to deal with skilled operations in a deforming environment. 
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Background 

In a deforming anatomic environment, minimally invasive surgery (MIS) therefore re- 
quires more sophisticated skills for the surgeons, the motion of an instrument suffers 
from complex geometrical and dynamic constraints. One approach for enhancing the 
accuracy and safety of human operation is to use robot controller to regulate the in- 
strument motion under either hands-on or teleoperation control These are referred to 
as virtual fixtures (VF), which is kind of task-dependent and computer-generated 
mechanisms to limit movement into restricted regions [1-3] or influence movement 
along desired paths [4-8]. Extension of VF into deforming environment also refers to 
dynamic virtual fixture [9], where the geometric constraint moves continuously, as a re- 
sult of changes in the physical environment or task being undertaken [10,11]. 

An important case of VFs for surgical application is guidance VF, Bettini et al. [5,6] used 
vision information to assist VF construction, and they discussed the application in vitreor- 
etinal surgery. Marayong et al. [7] demonstrated a geometrical constraint with varying 
compliance, which is described for the general spatial case. Compliant control is signifi- 
cantly necessary to overcome the uncertainties associated with registration errors, varia- 
tions in anatomy [12]. Li et al. [13] presented an anatomy generated VF for sinus surgery, 
which employed a constrained optimization framework to incorporate task goals, 
anatomy-based constraints, forbidden zones, etc. Castillo-Cruces et al. [14] presented an 
admittance controller with autonomous error compensation, a clear distribution of re- 
sponsibilities between surgeon and robotic system is preserved. Becker et al. [15] derived 
a virtual fixture framework for active handheld micromanipulators, the forces were re- 
placed with an actuated tool tip that is permitted by the high-bandwidth position mea- 
surements. These work all used admittance controller to implement VFs, the active robots 
comply with the manual force that applied on the end-effector or telemanipulated joystick. 
However, such first-order admittance controller with manual/autonomous error compen- 
sation is inadequate to adapt to dynamic environment. Bebek et al. proposed an intelligent 
control algorithm for robotic-assisted beating heart surgery, the robotic tools actively can- 
cel the relative motion between the surgical instruments and the point of interest on the 
beating heart [16]. However, motions along the preferred directions are not regulated. 

Traditional VFs deal with rotation and translation separately in space, the inter- 
connection between them is not involved in VF design. Synergy of orientation and pos- 
ition is important for MIS, it requires that the orientation should be ready before the 
position is reached. Otherwise, a delay is needed to instrument motion when the orien- 
tation and position are out of synchronous, it could be awful for real-time operation, 
collisions could probably appears in the inner anatomic operation space. Bullo and 
Murray [17] proposed a generalized proportional derivative (PD) laws on Euclidean 
Group 5£(3), which used logarithmic feedback control and dealt with rotation and 
translation simultaneously with a compact form. Besides, the geometric properties of 
Lie group and Lie algebra facilitate spatial compliance and stiffness matrix synthesis 
and decomposition. Shuguang Huang [18,19] examined the structure of spatial stiffness 
by evaluating the eigenscrew featured rank-1 primitives stiffness matrices. 

In our previous works, we studied the spatial compliance/stiffness matrices synthesis 
for admittance and impedance controlled devices respectively [20]. 

The proposed VFs could not effectively eliminate the tracking error drifts over the 
deforming frequency growth, thus we hope to improve the dynamic performance of the 
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VF for admittance controlled device. In this paper, we study the application of VF in 
deforming environment, and propose a novel framework of DVF for admittance-type 
device on SE{3), We construct the holonomic constraint/non-holonomic constraints 
and then search for the corresponded reference tasks to make a distinction between 
preferred and non-preferred directions. Different control strategies are employed to 
deal with the tasks along the distinguished directions. The paper is organized as fol- 
lows: the first section outlines the mathematical preliminaries of Lie group and Lie al- 
gebra for robot control, spatial compliance matrix synthesis and DVF on SE{3) for 
admittance-type device are proposed in Methods the last two sections describe a 
hardware-in-the-loop experiment and discussions. 



Methods 

Mathematical preliminaries 

The configuration space of a robotic system is defined on the special Euclidean group 
SE{3) and its subgroups, position and orientation of the end-effector correspond to a 
element of matrix Lie group. The Lie algebra of SE{3) is denoted as se{3). We express 
a group element g - (R,/?) ^SE{3) and velocity V = {o)^v)^se{3) using homogeneous 
coordinates as 



(1) 



the operator": R^^5o(3) is a cross-product matrix that transforms cross-product oper- 
ation into matrix multiplication, so that xy = y for all x, y^ R^. Elements of se{3) 
can also be represented as vector pair (w, v), 6>, v e R^. 

For all g e SE{3) and all X, Ye5e(3), the Adjoint map Kdg : se{3)^se{3) is a coordin- 
ate transformation on 5^(3), this allows us to transform spatial velocity from one coord- 
inate to another. The Lie bracket is given by the matrix commutator adx, which 
generalizes the standard cross-product operation in se{3). 
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Writing X as column vector pair (o), v), the 6x6 matrix representation of adjoint 
transformation and Lie bracket are 
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Spatial moment- force pair (m^f) m^feR^ are elements of the dual space of se{3)y 
which is denoted as 5e(3)*. The dual Adjoint map Ad* : se{3) * -^se{3)^ and Lie 
bracket ad^ are given by 
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(5) 



Let V2 = {o)2y ^2) ^ se{3) denote the spatial velocity of a rigid body in frame M2, g^ ^ SE(3) 
represents the right coordinate transformation from frame Mi to frame M2, 
and then the spatial velocity relative to frame Mi, denoted by Vi = {cji, Vi), is given by 
Vi = Adg^^ V2 . Let F2 = {m2,f2) ^ se{3) * denotes the moment- force pair acting on a rigid 
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body with respect to frame M2, gi2 ^ SE{3) represents the right coordinate transform- 
ation from frame Mi to frame M2, and then the spatial force in frame Mi is given by F2 
= Adl Fi, 

912 

On SE{3) and its proper subgroups, the exponential map exp: se{3) SE{3) is a sur- 
jective map and a local diffeomorphism [21]. Given ij/e so(3) and X = (if/^q)^ se(3), we 
have 



exp5o(3)(^) M^)^ 
0 1 



exp5o(3)(^) =1+ sin||^||^+(l-cos||^|P ^ 



exp5£(3)(X) 



(6) 
(7) 
(8) 



Equation (6) is also known as Rodrigues formula. The logarithmic map log: SE{3) 
se{3) is the inverse operation of exponential map. Let {R,p)eSE{3) be such that tr 
(R) ^-h then 



^''^soi3)(^)=^{^-'^^)^^o{3) 



^50(3) V 

in which 0 satisfies 0 = ^ (^r(R)-l) and |0| <m Also 

logsE(3)(R./^) 



0 0 



(9) 



(10) 



where ij/ = log5o(3)(R). 

Note that element of the Lie algebra can be expressed as a velocity as in Eq.(l) or the 
logarithm coordinates in Eq. (10). We denote them with V = (6>, v) in the first case, 
and with X = {ij/^q) in the second case. According Balls screws theory, every rigid co- 
ordinate transformation corresponds to a finite screw motion [22]. More precisely, the 
twist corresponds to the spatial velocity V = (^t>, v) of the Lie algebra, a screw refers 
to the exponential coordinate X = {ij/^q)^ The screw is an element of the 1-dimen- 
sional projective space of 5e(3), while a screw system with k degree of freedom 
(DOF) is a k-dimensional subspace [23]. The distance between the configuration 
state g and the identity ec = l^SE{3) is given by the norm of the logarithmic coord- 
inate, which is an Ad-invariant metric on the matrix Lie group G by either left or 
right transformation [24]. 



Il5llc 



;iog(g), \og{g)fl^ = -Jtr[\og{gY\og{g)\ 



(11) 



Dynamic constrains and reference search 

In deforming anatomic environment, the motion of instrument suffers from complex 
geometrical and dynamic constraints, their translational and rotational degree of free- 
dom are restricted. MIS operation requires more sophisticated skills for the surgeons, 
making a distinction between and preferred and non-preferred directions and employed 
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different control strategy helps reduce operation burden for surgeons. This can be 
achieved by construction of the holonomic/non-holonomic constraints and searching 
for the corresponded reference trajectories. The surgeons have complete control over 
the preferred directions, which are featured by a set of unit motion screws; while the 
deviation relative to the reference can be compensated autonomously. For a fully actu- 
ated control systems (the number of independent control inputs equals to the number 
of position variables), the reference trajectory falls into the holonomic and non- 
holonomic according to the constraints applied to the end-effector. 

(i). Holonomic constraint and the corresponded reference 

In holonomic constraints, the DOF of reference equals to the dimension of SE{3), mo- 
tion of the end-effector is strictly constrained. The task sequence is defined as a gener- 
alized time-varying curve g^(A, t) e X e R, Translation and rotation are continuous in 
X-t plane, the first-order partial derivatives for t and X exist. The temporal velocity 
with respect to inertia frame refers to 



yd{X,t)=^g;\ V,ese(3) (12) 



The tangent screw 



yt{X,t) = ^g;\ V,ese(3) (13) 



in inertia frame represents the preferred direction of instrument motion, which is inde- 
pendent of the choice of parameter A. Given the current configuration g of the end- 
effector, the instantaneous desired configuration on the task sequence g^(A, t) refers 
to the one with minimum norm 

eiX)=g,iX,t)-'git)\, 

I n4) 

gd = arg min \\e{X)\\Q = arg min - V2^^^ + 



in which ^og^^^^^{e) = {{//,q)&se{3) , the norm of logarithmic error is a weighted dis- 
tance of pure rotation and translation. Searching for the nearest reference along the 
task sequence can be generalized as the traversal searching the minimum point along 
the whole task sequence for initialization, local searching along the direction of norm 
reduction and the minimum is always reachable due to continuous restriction on rota- 
tion and translation. 



(ii). Non-holonomic constraint and the corresponded reference 

In non-Holonomic constraints, the DOF of reference is less than the dimension of Lie 
group, such as pure translation R^, rotation S0{3), or other displacement subgroups, 
motion of the end-effector is partially constrained. For example, instrument motion on 
a deforming surface while the tool shaft along the norm direction, which refers to the 
displacements in SE{2), Given the reference {[j^x> ^z\> Pr)> fiPr) = 0 and current 
state g = {[rixy viy, n^,p), the task is to make n^^m^, p py. Any rotation about axis z 
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cannot change the direction of axis-z, the instantaneous reference is the one with 
minimum norm in all possible frames gr5z(^)) 

9r{0)=grgM .... 

e{e)=g,{e)-'g 

Qd = arg min^ 1^(19)11^ = g,(<9)| tr(e(0))=4 

When the axis vectors and coincides, the rotation matrix of reference equals 
to that of robot state g according to Eq.(15), that means the user has complete control 
over the robot along the tangent plane and directs the task flow intuitively, the devia- 
tions relative to reference can be compensated manually or autonomously during 
human-robot cooperation. 

Spatial compliance for virtual fixture on SE(3) 

Admittance control facilitates VF by allowing the end-effectors to comply with the 
manual force anisotropically. The compliant behavior of admittance VF is featured by a 
6x6 symmetric positive semidefinite (PSD) compliance matrix Cyp * se{3) * se{3), 
which maps spatial force into small displacement [25,26]. The preferred and non- 
preferred directions construct the geometry constraints, both can be represented by 
unit screws in se{3). Given a set of linearly independent allowable screw set S in body- 
fixed frame, the task dependent compliant matrix Cyp can be decomposed into sum of 
m rank-1 PSD matrices, 

S = {Si}, Si = [ilfJ,qjY,i = 1, ...m<6 

C^P = CiSiS'^ + C2S2S^ + . . . + c^S^Sl.Ci > 0 (16) 

= Ci + C2 + • • • + Cm 

positive scalar q defines the compliance for each primitive compliance matrix Q. A 
wrench applied about the screw yields a twist deformation along the same screw, high 
compliance is encountered along the preferred directions, while low compliance along 
the non-preferred directions. Making a distinction between and non-preferred direc- 
tions helps reduce operation burden of surgeons, they only focus on operations along 
the preferred direction. Any deformation SX^ = {S(p^, Sq^) complying with the manual 
force ft = i^^yf) can be linearly represented by motion screws of S 



5q^ 



(17) 



The term i=t is manual force acting on the end-effector with respect to body frame, 81^ 
refers to the dual vectors of translational deformation and rotational deformation in body 
frame. A complete constraint corresponds to a full-rank compliance matrix C, a deficient- 
rank matrix indicates an incomplete constraint. The compliance matrix Cyp is defined in 
body- fixed frame 'b\ if ^ e SE{3) represents the right coordinate transformation from frame 
to frame 'd, then the compliance matrix Cyp in the new coordinate frame 'd is given by 

C^P=A^^-iCtpA4, (18) 



A customized virtual fixture for complicated surgical tasks can be treated as the com- 
bination of one or more primitives [27]. In Table 1, we lists the preferred motion 
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Table 1 Four primitive tasks and the corresponding preferred motion screw 

Primitive tasks Allowable motion screw set 

Fixed configuration g^ e 5^(3), S = log (gd^g) 

Trajectory tracking ^ SE{3) ~ bg (g^^ g) 

Rotate around a line S = (s,r x s) s: rotation axis, r: arbitrary point on s 

Move on plane Si = {0,V]),S2 = {0,vi),S^ = (s,r x s) s\^m{v^,v^: continent plane, s: norm direction 



screws for four static primitive tasks. The featured screw system could be redundant, 
twist matrices intersection is a method to reduce the redundancy of multiple allowable 
screw sets [28,29]. 

Dynamic virtual fixture for admittance-type device 

Considering that the end-effector is of admittance type, which can be modeled as non- 
backdrivable kinematic system, and all joints are equipped with velocity-source actua- 
tors. The VF on admittance type device acts as anisotropic compliant wall, the end- 
effector complies with the manual force along preferred the directions. Bettini and 
Marayong [5,7] combined the deviation error into the allowable motion subspace and 
redefined a new VF that makes it possible to reduce the deviations manually. However, 
it has been shown that manual compensation does not necessarily compensate for all 
deviations, especially when the VF is defined for translation and the deviation error is 
of orientation. Castillo-Cruces et al. [14] added linear error feedback term into the ad- 
mittance controller to compensate rotation and translation deviations separately and 
autonomously along the non-preferred directions. 

However, the first-order admittance controller with manual or autonomous error 
compensation is inadequate to adapt to dynamic environment. This section extends 
these VFs with dynamic tracking into a deforming environment on 5'£(3). The 
admittance-type robot can be modelled as a left invariant first-order fully actuated sys- 
tems on SE(3), and satisfies the quasi-equilibrium condition = 0. 



= (w^V^)G5e(3) 



K,CvF(g,g^) 



(19) 



The VF is defined as task-dependent and time- varying compliance matrix Cyy{9t9^)> 
Cyp , velocity and manual force = (m^,/^)G5e(3)* acting on the end-effector 
are represented in body- fixed frame. Admittance gain =diag{Cy,Isx3> Cpl^xs) controls 
the compliance of the robot to user input, choosing Cp low imposes additional resist- 
ance along the preferred directions, high compliance could result in fast robot motion. 
Velocity of end-effector is controlled jointly by the admittance controller and dynamic 
tracking controller Udyf^^ to make the robot state g track the reference g^, which is 
described by = V^g^. The deviation of robot state g relative to the reference with 
respect to the reference frame is defined as e = gd^g, the matrix form is given by 



RjR R^,(p-p,) 
0 1 



(20) 
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It is also called natural error and independent of the choice of inertia frame [17]. An- 
other equivalent definition is = g'^Qdy the deviation is seen in body frame. These two 
error definitions and their logarithmic coordinate are mutually invertible. 



log(e) = -log(e'') 



(21) 



Define the configuration error e = eV^ as the first case, the error velocity with respect 
to inertia frame is given by 

Ve = -Adg-.Vd^V^ (22) 

We design the dynamic tracking controller with logarithmic error feedback in matrix form 

KvF^g^ 9d) = Adg-. V,-Kp log(e) (23) 

where Kp = kpO^, =diag{krl3x3> hx3)> kp, kr > 0, the linear coefficient kp controls the 
speed of error convergence, maximum choice of kp is restricted by the actuation power 
of motors, the rotation-translation (R-T) ratio kr controls the synchronous the rotation 
and translation. We take this control law into the robot kinematics and cancel out 
manual effect, the error control function of the close loop system satisfies 



Ve + Kp\og{e)=0 



(24) 



The control law exponentially stabilizes the configuration error e at identity I from 
any initial configuration. Let X = log {e{t)) = {i//, q) e se{?>) be the logarithmic coordinate 
of e{t), we can relate the column vector form X and through 



X 



E 



(-1)% 



ad« (n) = 



(25) 



where the {Bn} are Bernoulli numbers, the symbols Bx denote the Lie bracket series 
[17]. We have 



0 




0 









X 



By kJ<r 



{kp—kpkr 



-kpkrX— (kp—kpkr) 



0 

A{xlf)-^q 



Separating the rotational part and the translational parts 
^ = -kpkr^ 
q = —kpkr(j—kp7Zii/q 



(26) 
(27) 



the effect of rotation on the translation is featured by operator DJ^, which is complex 
and highly dependent on the rotation axis i//. We can adjust the interconnection between 
rotation and translation by choosing R-T ratio kr. The rotation part is exponentially stabi- 
lized for all initial rotation matrix R(0) such tr(R)7^-l. Regarding the translational part, 
consider the candidate Lyapunov function = ^ ||^||^. Its time derivative satisfies 
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dt 



W = -(q, kpkrq + {kp-kpkr)A{i//) q 



-/<Alkf-(VV<r) k|| ^a{\\ilf\\)\\q_ 



< 0 



where a{y) = (y /2) cot (y/2),0<\\i//\\ < I, q = q\\-^q _i is the orthogonal decomposition of q 
along span {i//} and Thus global exponential stability is proven also for translational 

part. 

Any deviation of the end-effector from the reference is compensated autonomously. 
This error compensation is regarded as a target tracing acting on the virtual fixture 
[14]. The discrete realization of the VF controller usually introduces time delay and dy- 
namic tracking errors, forecast of the reference velocity helps improve the tracking 
precision. As seen in Figure 1, the instantaneous reference and allowable motion set 
are defined from multiple geometric constraints; manual operation is regulated by the 
compliance controller, which is synthesized by the allowable motion set. The operator 
has complete control along the preferred directions, fast motion along the non- 
preferred direction and deviations relative to the reference are compensated autono- 
mously. The dynamic tracking term in the DVF controller helps the surgeon to deal 
with operations in deforming environment. 

Experiments, results and discussion 

The proposed DVF is designed for fully actuated robotic system with force/torque sen- 
sor. However, both force/torque sensor and admittance controlled device are not avail- 
able. We designed a virtual proxy based hardware-in-the-loop simulation test bed to 
evaluate the proposed DVF. The PHANToM Omni device, from SensAble Technolo- 
gies, is equipped with 3 degree-of-freedom (DOF) actuations and 6 DOF position mea- 
surements. It serves as a force source rather than a haptic device to drive a virtual 
admittance-type proxy alternatively. The device is actuated passively by human force 
that applied on the end-effector, while there is no haptic feedback to the operator. 



The kinematics equation of the admittance-type proxy refers to 
9p 



--9pK 



(28) 
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Figure 1 Control loop of DVF for admittance-type device. 
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the proxy is controlled by the DVF controller in Eq, (19). The proxy complies with the 
actuation force acting on the proxy with respect to body-fixed frame, which is esti- 
mated from the robot dynamics of the PHANToM device. 



f{e)F' = M(0)0-N(0, 0)0 



: AdlF' 



(29) 



where M{0) is the inertia matrix, N(0, O) is the Coriolis matrix, the gravity term C{0) 
is eliminated from the actual manual force, kinematics and dynamics details of the 
Phantom Omni device can be found in [30]. represents the manual force with re- 
spect to the inertia frame, J(0) is manipulator Jacobian matrix, U^yf^^ refers to the dy- 
namic tracking controller in Eq. (23). 

The real haptic device and the virtual admittance-type proxy are integrated in the 
MATLAB/Simulink environment by the PHANSIM TOOLKIT. The overview of the 
hardware-in-the-loop test bed for DVF implementation on admittance-type proxy is il- 
lustrated in Figure 2. The control loop of the haptic device runs at 1 kHz, we use the 
PHANToM clock to down sample the joint measurements to 200Hz. The joint angles 
0 of the PHANToM Omni are processed by a low-pass filter and a differentiator suc- 
cessively to estimate the joint velocity 0 and acceleration 0, manual force are esti- 
mated by the Phantom robot dynamics in Eq. (29). The proxy state is updated by 
proxy body velocity discretely, which is jointly controlled by admittance controller 
and dynamic error compensator Eq. (28). The proxy motion is constrained on a 
deforming sphere to simulate the beating heart surgery environment. We use an exter- 
nal clock to update the reference state g^, proxy state and the deforming radius. For 
fully apprehension, a 3D virtual environment is built up and visualized using the 
MATLAB Virtual Reality Toolbox. Both the proxy and the reference are modelled as 
rigid pens with unit isotropic inertia matrix. 

In the simulation experiment, we simplif)^ the admittance controller robot as a virtual 
proxy, manual force is estimated indirectly through robot dynamics rather than a real 
force sensor. The proxy workspace is not restricted compared to a real robot, measure- 
ment errors of joint angles are not involved, manual force is also inaccurate due to in- 
ertia parameters estimation. We will evaluate the proposed algorithm on a real 
admittance controlled device when all instruments are ready in the future. 



Haptic 
device 



Phantom 
clock 




Low-pass 
filter 



0 



Differentiator 



0,0 



Kinematics 
Dynamics 



Dynamic Constraints 



Dynamic 
Tracking 



Reference 



Logarithmic 
error feedback 



DVF 
Controller 




Deforming control 



Figure 2 Hardware-in-the-loop test bed for DVF simulation on admittance-type proxy. 
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Numerous surgical tasks require the surgeon to follow a predetermined path on a de- 
formable anatomy tissue surface while maintaining the shaft orientation within a safe 
range, such as minimally invasive beating heart surgery, the area of interest deforms 
with heartbeat and respiratory motion, instrument motion should be regulated to sat- 
isfy the varying geometric constraints. We simplified this task as a non-holonomic con- 
straint problem by moving a tool on a deforming ball, while the tool shaft along the 
radial direction, the axial rotation is not constrained. The radius r of the ball varies 
periodically, r = Tq -\- Ar sin cot y Tq = 15 mm, Ar = 5 mm. The bandwidth of the position 
measurements is 26 Hz, which refers to the upper limit of tissue motion resulted from 
physiologic movements, such as heartbeat and respiration [16]. 

The instantaneous reference refers to the one with minimum norm in Eq. (14). 
The allowable motions on the deforming sphere refer to translation along the tangent 
directions and rotation around the radius direction, the corresponded allowable motion 
set S is defined in the inertia frame. 



where r^, Vy, are unit axis vectors of g^, Pd is the translation part. The compliance 
matrix Cyp is synthesized by the allowable motion screw set S. The workspace of the 
Phantom Omni device is 1:1 mapped into the proxy workspace, the corresponding co- 
ordinate axes are parallel and without rotational transformation. The initial state of the 
proxy tool is set to be the inkwell-calibrated configuration. The translation and orienta- 
tion errors of the tool with respect to the reference are defined as 



Rotational error Cy. describes the angle of the rotation about the equivalent axis v|/, 
translational error Bp refers to the distance between tool tip p and reference p^^ We use 
tracking error and following index to evaluate the performance of the proposed dy- 
namic DVF in deforming environment. Translational tracking error (TTE) and rota- 
tional tracking error (RTE) refer to the root-mean-square of translation and orientation 
errors respectively during the human-robot cooperation. The following index repre- 
sents how quick and how well the proxy can follow manual operation, or how much 
manual input can be transferred to the task related commands. More specifically, 
the following index is defined as the cross-correlation coefficient of manual force 
= {m^yf) and corresponded velocity acted on the proxy = (o^o i^c)> which consists 
of the admittance controller output and the logarithmic error feedback in Eq. (22). The 
rotational following index (RFI) and translational following index (TFI) for admittance- 
type device are defined as 



Si = {0,r^),S2 = {0,ry),S3 = {r^,p^ x r^), S = {5:1,5:2,5:3} 



e = g-/g={R^,RXdip-Pd)) 

ep = \\P-Pd\\ 

er=\M 



(30) 




(31) 
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TFI = corr(v^,/^ 



E(-c-c)'(/'-/) 



El 



(32) 



Three experiments were conducted to validate the efficiency of proposed DVF for 
admittance-type proxy. 



Proportional feedback control experiment 

In this experiment, we compare the performance of three control laws to stabilizes the 
robot state g at fixed state from any initial state g{0) = {R{0),p{0)), The deviation of 
the end-effector state relative to the reference is defined as e = gd^g, the logarithmic co- 
ordinates refers to log(e) = {i//^q)e$z{3), 
1). Separate feedback 



P-Pd 









kwhx3 0 




y. 




0 /TvIsxS. 


2), Double-geodesic feedback 




'to"' 




kwhx3 0 




y. 




0 kyl3x3_ 


3). Logarithmic feedback 








kwhx3 0 








0 ^^3x3 



Law-1 takes the error definition and feedback control from Castillo-Crucess work 
[14], which treats translation and rotation separately. Law-2 applies proportional feed- 
back actions along geodesic directions for both rotation and translation in S0{3) and 
respectively, law-3 is the logarithmic feedback control. Law-1 is dependent on the 
choice of inertia frame, both law-2 and law-3 are independent of inertia frame choice. 
To compare the proportional feedback laws presented above, the same planar motion 
SE{2), initial state go, destination state g^ and proportional parameters k^, are set for 
the three control laws in our experiment. 

As seen in Table 2, there are not obvious difference in the residual errors of the three 
laws, expect for their behaviors during the convergence period. All three control laws 
converge at the same rate in rotational part, as seen in Figure 3, law-1 shows more 
curved behaviour and takes the longest path to approach the fixed state, law-2 follows 
the shortest path. The performance of Law-2 deteriorates for 2^^ order robotic sys- 
tems, only the logarithmic feedback control could locally exponentially stabilized the 
robot state at I, more detail comparison can be found in [17]. We take the logarithmic 
feedback law in our DVF controller design, with which the robot performs steadily 

Table 2 Residual errors of three proportional feedback control laws 





Law-1 


Law-2 


Law-3 


(mm) 


0.0046 


0.0050 


0.012 


RTE (rad) 


2.7108e-020 


1.7837e-017 


1.2123e-007 
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3 4 5 

Time (sec) 



Figure 3 Three proportional actions on SE(2) for 1st {cinematics system with kw = 2, ky=^, (a) 
translation error, (b) convergence trajectories, each point is depicted as a frame. 



stable, both the rotational and translational part converged uniformly to zero, no 
oscillations appeared. 

We also analyzed the influence of proportional parameter kp of the logarithmic error 
feedback controller. The proxy is asked to approximate the nearest reference on the 
sphere autonomously and manual excitation is still. The initial state of the proxy is set to 
be the inkwell- calibrated position. As shown in Figure 4, kp controls the speed of error 
convergence, maximum choice of kp is restricted by the actuation power of motors 

Dynamic tracking experiment 

To discusses the dynamic properties, the operator dragged the gimbal pen of haptic de- 
vice back and forth along x-axis in the inertia frame, the tangent components of 



100 




4 6 

Time (sec) 

(b) 

Figure 4 Influence of kp on error convergence with deforming frequency 2 rad/s, R/T ratio =^, (a) 
translational error, (b) rotational error. 
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manual force drove the proxy move along the circle of latitude on the deforming 
sphere with frequency varies from 0 to 40 rad/s, R-T ratio =1, =0.1 rad/s, 
Cp =100 mm/s. As shown in Figure 5, the TTE increases linearly with deforming fre- 
quency growth, the impact of kp on the dynamic error is not clear, but the impact of 
dynamic compensation (DC) is obvious. The RTE is suppressed by linear logarithm 
error compensation, and is independent of deforming frequency. This is due to the 
fact that orientation of reference remained constant while translation along the radial 
direction, translational deviation along the same direction is compensated autono- 
mously; the proxy is actuated manually along tangent directions, the referred orienta- 
tion varied accordingly. The discrete realization of the DVF controller usually 
introduces time delay and dynamic tracking error, forecast of the reference velocity 
helps improve the tracking precision. 
As seen in Figure 5, the TTE of the VF with DC is smaller than the VF without DC; 
furthermore, the VF with dynamic forecasting compensation (DFT) performs much 
better than the VF with DT. The DVF can improve the dynamic properties in low- 
frequency range, its dynamic performance deteriorated slowly with deforming fre- 
quency growth, this is due to the first-order kinematics property of the admittance-type 
device. R-T ratio can affect the synchronous of rotation and translation as well as the 
tracking error convergence speed because of interconnection between rotation and 
translation on SE{3), We recorded the TTE and settling time with deforming frequency 
40 rad/s, kp =20, admittance gains =0.1 rad/s, Cp =100 mm/s, R-T ratio was set as 
4,3,2,1,0.5 respectively, the results are listed in Table 3. Appropriate settings of R-T ra- 
tio help speed up the error convergence and reduce error level. 




0.05 



^kp=2 with DC 
-^kp=5 with DC 
-e-kp=10 with DC 
-^kp=10 without DC 
-0-kp=1O with DFC 



10 15 20 25 30 
Deforming Frequency (rad/sec) 



(a) 













^kp=2 with DC 
-B-kp=5 with DC 
-e-kp=10 with DC 
■■1^-kp= 10 without DC 
O- kp=10 with DFC 















10 15 20 25 30 
Deforming Frequency (rad/sec) 



40 



(b) 

Figure 5 Results of dynamic traclcing experiment witii difference kp settings and different tracl<ing 
strategies (without dynamic compensation (DC), with dynamic compensation, with dynamic 
forecasting compensation (DFC)), (a) translational tracking error; (b) rotational tracking error. 
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Table 3 Impact of R/T ratio on dynamic tracking error 



R-T ratio 


4 


3 


2 


1 


0.5 


Tracking error(mm) 


0.5347 


0.5996 


0.6487 


0.6610 


0.6569 


Settling time(sec) 


0.1 


0.2 


0.35 


0.6 


1.3 



Human-robot collaboration experiment 

This experiment is designed to analyze human factors on human-robot collaboration 
with assistance of the DVF controller. To analyze the influence of manual force on the 
tracking error, three people took part in this experiment. The operator applied time- 
varied force along x-axis with respect to inertia frame, manual force and tracking error 
are depicted in Figure 6(a-b); the operator pivoted the gimbal pen around arbitrary axis, 
the tool shaft orientation deviated from the reference accordingly, manual torque and 
the tracking error are plotted in Figure (c-d). 

As seen in Figure 6, changes in the magnitude and direction of manual force had in- 
fluence on rotational and translational errors. The operator dragged the tool along 8- 
shaped curve with three intensities of manual force{low, medium, high}, kp =10, 
deforming frequency 5 rad/s, the following index RFI and TFI, dynamic tracking error 
TTE and RTF are recorded in Table 3. 

As seen in Table 3, the dynamic tracking error is linearly related to manual force in- 
tensity and R-T ratio, the compliances Cp play the same role as they scaled the man- 
ual force intensity that acted on the proxy. Faster manual operation resulted in larger 
dynamic tracking error. The proportional gain of the rotational part is multiplied by R- 
T ratio. The VF controller dominates the rotation over manual operation when large R- 
T ratio is set, the angular velocity resulted from human input is less than that from the 
logarithmic error feedback controller, and then the rotational following index decreases 
accordingly. A fair distribution on rotation control between the autonomous controller 
and human operation is achieved when the R-T ratio is low; the rotational following 
index has little change. The translational following index is negatively related to the 




Figure 6 The tracking error of multiple manual operations with kp = 2, (o = 5 rad/s, R-T ratio = ^, 
Cr = OA rad/s, Cp= 100 mm/s; (a) manual force when moving along axis-x; (b) resulted error of 
operation (a), (c) manual torque when pivoting around arbitrary axis; (d) resulted error of operation (c). 

V J 
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R-T ratio, this is because the impact of rotation part on the translation is weakened 
when small R-T ratio is set. As seen from Tables 3 and 4, we can also conclude that 
higher R-T ratio helps suppress the dynamic tracking error, however it could decrease 
the rotational following index during human-robot cooperation, choosing R-T ratio 
small can increase the tracking error and the translational following index, the choice 
of R-T ratio should compromise between user experience and the tracking error. More 
specifically, the inter-relationship between the components of manual moment-force 
pair (m^, f^) and the output velocity (wo ^c) featured by the correlation coefficients 
of and The operator is asked to drag the tool randomly on the deforming sphere 
for five minutes with deforming frequency 5 rad/s, kp =10. All the data are standardized 
to eliminate the variable dimension difference. The Pearson product-moment correl- 
ation coefficients are calculated for all one-to-one element combinations in and Vc 
with confidence interval 99% [31]. 

As seen in Table 5, the difference of correlation coefficient reveals that the DVF con- 
troller has stronger control over the manual operation in tool shaft orientation. Rota- 
tion (Oz round the tool shaft and translation v^, Vy along tangent directions are strictly 
linear correlative to manual input m^, fj,, fy respectively, the operator has completely 
control over these direction. The correlation between manual moment m^, my and tan- 
gent velocity v^, Vy are notable due to the coupling between rotation and translation. 
Velocity v^ is independent of the manual input; the DVF controller takes charge of the 
deviation compensation along the deforming direction. 

The trajectories of the operator driving the proxy along straight line and circle on the 
deforming sphere are illustrated in Figure 7, the complicated behaviors of dynamic task 
is beyond human control, making a distinction between the preferred and non- 
preferred directions and employed different control strategy helps reduce operation 
burden of surgeons and improve their skills in dynamic tasks. 

Conclusions 

This paper described a novel framework of DVF for admittance -type manipulators on 
the Euclidean Group SE(3) to assist the surgeons to deal with the dynamic tasks, which 



Table 4 Results of shape-8 operation with assistance of DVF on admittance proxy 



Intensity 


R-T ratio 


RFI 


TFI 


RTE (rad) 


TTE (mm) 


Low 


4 


0.1296 


0.6148 


0.0093 


0.1046 


Low 


2 


0.8138 


0.8333 


0.0260 


0.1693 


Low 


1 


0.6554 


0.8647 


0.0575 


0.2633 


Low 


0.5 


0.8008 


0.9087 


0.0856 


0.3404 


Medium 


4 


0.1815 


0.7453 


0.0180 


0.1567 


Medium 


2 


0.7633 


0.9374 


0.0452 


0.1876 


Medium 


1 


0.6231 


0.8815 


0.0712 


0.2847 


Medium 


0.5 


0.7000 


0.9407 


0.1226 


0.3455 


High 


4 


0.1692 


0.8519 


0.0291 


0.1140 


High 


2 


0.8190 


0.9440 


0.0727 


0.1832 


High 


1 


0.7173 


0.7725 


0.0812 


0.2728 


High 


0.5 


0.6133 


0.8714 


0.1755 


0.3303 
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Table 5 The correlation between manual force and proxy velocity 



Corr. 




nriy 


mz 


fx 




fz 




0. 761 


-0.192 


-0.051 


-0.003 


-0.940 


-0.692 


OOy 


-0.046 


0.880 


0.092 


0.922 


-0.058 


0.114 


a3z 


-0.545 


-0.043 


1.000 


0.173 


0.048 


0.021 


Vx 


-0.127 


0.889 


0.134 


0.998 


-0.040 


0.106 


Vy 


-0.754 


0.127 


0.091 


-0.044 


0.998 


0.676 


Vz 


0.006 


0.039 


0.016 


0.079 


-0.029 


0.012 



Pearson's correlation coefficient with confidence interval 99%. 



unites rotation and translation in a compact form We constructed the holonomic/ 
non-holonomic constraints, and then searched for the corresponded references to make 
a distinction between preferred and non-preferred directions. Different control strat- 
egies are employed to deal with the task along these directions. The DVF can improve 
the dynamic properties of human-robot cooperation in low-frequency deforming envir- 
onment, and maintain synergy of orientation and translation during the operation. The 
experiments show that the DVF implemented on the virtual admittance -type proxy can 




X(mm) 

(b) 

Figure 7 Trajectories of the proxy along straight line (a) and circle (b) on the deforming sphere 
with kp = 5, (0 = 5 rad/s, R-T ratio = 1, = 0.1 rad/s, Cp = 1 00 mm/s. 
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assist the user to deal with the skilled operations in deforming environment. We will 
evaluate the proposed algorithm on a real admittance controlled device when all instru- 
ments are ready in the future. 

Notation and nomenclature 

a; scalar 

fi force vector 

m; moment vector 

0); angular velocity vector 

v; translational velocity vector 

R; rotation matrix 

0; null vector 

0; null matrix 

q; Vector 

q; cross-product matrix of vector q 
SE{3); Lie group 

SE{2); Displacement subgroup of Lie group 

S0{3); Rotation group 

se (3); Lie algebra 

50 (3); Lie algebra of rotation group 

5^(3)*; dual space of Lie algebra 

5o(3)*; dual space of so{3) 

g; elements in SE{3) 

V = (o), v); matrix form velocity in se(3) 

V= (o), v); column vector form velocity in se(3) 

X = {ij/,q); the matrix form logarithmic coordinates of^ 

X= {xj/y q); column vector form logarithmic coordinates of ^ 

* ^; element relative to the body frame 

* ^; element relative to the spatial frame 
Ad^, Ad*; adjoint transformation 
adx, ad^; Lie bracket 

exp SE{3){ * )> exponential map on SE{3) 

log5E(3)(*); logarithmic map on SE{3) 

II * II g; norm on the matrix Lie Group SE{3) 

Generally, we use italic small characters for scalars, italic bold for vectors , bold capi- 
tals for matrices, italic bold capitals for vectors of Lie algebra. 
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